/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/**
 * @file NDS03_ContiunousRanging_Example.c
 * @author tongsheng.tang
 * @brief 连续测量示例
 * @version 2.x.x
 * @date 2024-04
 *
 * @copyright Copyright (c) 2024, Nephotonics Information Technology (Hefei) Co., Ltd.
 *
 */

#include <stdio.h>
#include "nds03_platform.h"
#include "nds03_comm.h"
#include "nds03_dev.h"
#include "nds03_data.h"
#include "nds03_calib.h"
#include "nds03_def.h"

#include "zino.h"
#include "drv_common.h"
#include <rtconfig.h>
#include <rtthread.h>
#include "drv_pin.h"
#include "nds03_api.h"

#define DBG_TAG "ND03/usr"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

/** 模组设备 */
NDS03_Dev_t g_nds03_device = {
    .platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR,
    .platform.xshut_pin = PB6,
};

/**
 * @brief 单次测量接口
 *
 * @param pNxDevice 模组设备
 */
void SG_RangingTest(NDS03_Dev_t *pNxDevice)
{
    /* 获取测量数据 */
    NDS03_GetContinuousRangingData(pNxDevice);
    NX_PRINTF("depth1:%5d mm, confi:%5d, count:%5d\r\n", pNxDevice->ranging_data[0].depth, pNxDevice->ranging_data[0].confi, pNxDevice->ranging_data[0].count);
    NX_PRINTF("depth2:%5d mm, confi:%5d, count:%5d\r\n", pNxDevice->ranging_data[1].depth, pNxDevice->ranging_data[1].confi, pNxDevice->ranging_data[1].count);
    NX_PRINTF("depth3:%5d mm, confi:%5d, count:%5d\r\n", pNxDevice->ranging_data[2].depth, pNxDevice->ranging_data[2].confi, pNxDevice->ranging_data[2].count);
    NX_PRINTF("depth4:%5d mm, confi:%5d, count:%5d\r\n", pNxDevice->ranging_data[3].depth, pNxDevice->ranging_data[3].confi, pNxDevice->ranging_data[3].count);
}

/**
 * @brief NDS03连续测量主函数示例
 *
 * @return int32_t
 */
int32_t nds03_main(void)
{
    NDS03_Dev_t *pNxDevice = &g_nds03_device;
    /* 循环测距次数为100次 */
    // int32_t cnt = 100;

    /* 初始化平台，包含I2C初始化 */
    if (NDS03_ERROR_NONE != nds03_platform_init(&pNxDevice->platform, NULL))
    {
        NX_PRINTF("nds03_platform init error\n");
        return NDS03_ERROR_API;
    }

    /* 初始化设备变量的IIC地址 */
    pNxDevice->platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR;

    /* 循环等待设备启动, 若模组或者IIC读写函数有问题则会报错 */
    if (NDS03_ERROR_NONE != NDS03_WaitDeviceBootUp(pNxDevice))
    {
        NX_PRINTF("NDS03_WaitDeviceBootUp error\r\n");
        return NDS03_ERROR_BOOT;
    }

    /* 初始化模组设备 */
    if (NDS03_ERROR_NONE != NDS03_InitDevice(pNxDevice))
    {
        NX_PRINTF("NDS03_InitDevice error!!\r\n");
        return NDS03_ERROR_INIT;
    }

    /* 连续模式开启测量 */
    NDS03_StartContinuousMeasurement(pNxDevice);

    /* 循环测量 */
    while (1)
    {
        SG_RangingTest(pNxDevice);
        // rt_thread_mdelay(10);
        // LOG_D("cnt = %d", cnt);
    }
    /* 连续模式关闭测量 */
    NDS03_StopContinuousMeasurement(pNxDevice);

    /* 释放平台 */
    nds03_platform_uninit(&pNxDevice->platform, NULL);

    return 0;
}
int nds03_init(void)
{
    NDS03_Dev_t *pNxDevice = &g_nds03_device;
    /* 循环测距次数为100次 */
    // int32_t cnt = 100;
    // NDS03_Wakeup(&g_nds03_device);
    /* 初始化平台，包含I2C初始化 */
    if (NDS03_ERROR_NONE != nds03_platform_init(&pNxDevice->platform, NULL))
    {
        NX_PRINTF("nds03_platform init error\n");
        return NDS03_ERROR_API;
    }

    /* 初始化设备变量的IIC地址 */
    pNxDevice->platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR;

    /* 循环等待设备启动, 若模组或者IIC读写函数有问题则会报错 */
    if (NDS03_ERROR_NONE != NDS03_WaitDeviceBootUp(pNxDevice))
    {
        NX_PRINTF("NDS03_WaitDeviceBootUp error\r\n");
        return NDS03_ERROR_BOOT;
    }

    /* 初始化模组设备 */
    if (NDS03_ERROR_NONE != NDS03_InitDevice(pNxDevice))
    {
        NX_PRINTF("NDS03_InitDevice error!!\r\n");
        return NDS03_ERROR_INIT;
    }

    uint32_t frame_time = 0;
    uint32_t pulse_num = 0;
    uint8_t target_num = 0;
    NDS03_SetTargetNum(pNxDevice, 1);
    NDS03_GetPulseNum(pNxDevice, &pulse_num);   // 390000
    NDS03_GetTargetNum(pNxDevice, &target_num); // 4
    NDS03_GetFrameTime(pNxDevice, &frame_time); // 33333

    // NDS03_SetFrameTime(pNxDevice, 20000);
    // NDS03_SetPulseNum(pNxDevice, 1000);

    LOG_D("frame_time = %d, pulse_num = %d, target_num = %d\n", frame_time, pulse_num, target_num);

    /* 连续模式开启测量 */
    int8_t ret = NDS03_StartContinuousMeasurement(pNxDevice);
    if (NDS03_ERROR_NONE != ret)
    {
        NX_PRINTF("NDS03_StartContinuousMeasurement error!! %d\r\n", ret);
    }

    // for(int i=0;i<10;i++)
    // {
    //     SG_RangingTest(pNxDevice);
    //     rt_thread_mdelay(33);
    // }
    return 0;
}

int nds03_get_result(NDS03_Dev_t *pNxDevice)
{
    return NDS03_GetContinuousRangingData(pNxDevice);
}
/**
 * @brief 获取结果
 *
 * @param range 距离
 * @param confidence 置信度
 * @return int 0: ok, else: fail
 */
int nds03_get_result_data(uint16_t *range, uint16_t *confidence)
{
    NDS03_Error ret = NDS03_ERROR_NONE;
    // uint8_t     data_cnt;
    // uint32_t    retry_cnt = 20000;
    // NDS03_Dev_t *pNxDevice = &g_nds03_device;
    /* 等待测量完成 */
    // ret |= NDS03_ReadByte(pNxDevice, NDS03_REG_DATA_CNT, &data_cnt);
    /* 读取测量数据 */
    ret |= NDS03_ReadRangingData(&g_nds03_device);
    /* 清除数据有效标志位 */
    // ret |= NDS03_ClearDataValidFlag(pNxDevice);
    // pNxDevice->data_cnt = data_cnt;
    *range = g_nds03_device.ranging_data[0].depth;
    *confidence = g_nds03_device.ranging_data[0].confi;
    // if(data_cnt != pNxDevice->data_cnt)
    // {
    // }
    // NX_PRINTF("depth1:%5d mm, confi:%5d, count:%5d\r\n", g_nds03_device.ranging_data[0].depth, g_nds03_device.ranging_data[0].confi, g_nds03_device.ranging_data[0].count);
    return ret;
}
void nds03_test()
{
    NDS03_Dev_t *pNxDevice = &g_nds03_device;
    int ret =
        NDS03_ReadByte(pNxDevice, NDS03_REG_STATE, &pNxDevice->dev_pwr_state);
    NX_PRINTF("fdev_pwr_state: %X %d\r\n",pNxDevice->dev_pwr_state, ret);
}
MSH_CMD_EXPORT(nds03_test, nds03);
int nds03_continuous_ranging_init(NDS03_Dev_t *pNxDevice)
{
    /* 初始化平台，包含I2C初始化 */
    if (NDS03_ERROR_NONE != nds03_platform_init(&pNxDevice->platform, NULL))
    {
        NX_PRINTF("nds03_platform init error\n");
        return NDS03_ERROR_API;
    }

    /* 初始化设备变量的IIC地址 */
    pNxDevice->platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR;

    /* 循环等待设备启动, 若模组或者IIC读写函数有问题则会报错 */
    if (NDS03_ERROR_NONE != NDS03_WaitDeviceBootUp(pNxDevice))
    {
        NX_PRINTF("NDS03_WaitDeviceBootUp error\r\n");
        return NDS03_ERROR_BOOT;
    }

    /* 初始化模组设备 */
    if (NDS03_ERROR_NONE != NDS03_InitDevice(pNxDevice))
    {
        NX_PRINTF("NDS03_InitDevice error!!\r\n");
        return NDS03_ERROR_INIT;
    }
    uint32_t frame_time = 0;
    uint32_t pulse_num = 0;
    uint8_t target_num = 0;
    NDS03_SetTargetNum(pNxDevice, 1);

    NDS03_GetPulseNum(pNxDevice, &pulse_num);   // 390000
    NDS03_GetTargetNum(pNxDevice, &target_num); // 4
    NDS03_GetFrameTime(pNxDevice, &frame_time); // 33333

    // NDS03_SetFrameTime(pNxDevice, 20000);
    // NDS03_SetPulseNum(pNxDevice, 1000);

    LOG_D("frame_time = %d, pulse_num = %d, target_num = %d\n", frame_time, pulse_num, target_num);

    /* 连续模式开启测量 */
    NDS03_StartContinuousMeasurement(pNxDevice);
    return 0;
}
int nds03_stop_measurement(NDS03_Dev_t *pNxDevice)
{
    /* 连续模式关闭测量 */
    NDS03_StopContinuousMeasurement(pNxDevice);

    /* 释放平台 */
    nds03_platform_uninit(&pNxDevice->platform, NULL);
    return 0;
}
int nds03_power_down(NDS03_Dev_t *pNxDevice)
{
    /* 关闭电源 */
    NDS03_SetXShutPinLevel(pNxDevice, 0);
    return 0;
}
// int nd03_task_init()
// {

//     rt_thread_t nds03_thread = rt_thread_create("fc_error_handle", nds03_main, RT_NULL, 512, 10, 10);
//     if (nds03_thread != RT_NULL)
//     {
//         rt_thread_startup(nds03_thread);
//         LOG_I("nds03_thread create success.");
//     }
//     else
//     {
//         LOG_E("nds03_thread create failed.");
//     }
//     return 0;
// }
// ZINO_APP_EXPORT(nd03_task_init);

// #include "sensor.h"
// static rt_err_t nds03_control(struct rt_sensor_device *sensor, int cmd, void *args)
// {
//     rt_err_t result = RT_EOK;
//     NDS03_Dev_t *pNxDevice = sensor->parent.user_data;
//     switch (cmd)
//     {
//     case RT_SENSOR_CTRL_GET_ID:
//         if (i2c_device_try("i2c2", NDS03_DEFAULT_SLAVE_ADDR) == 1)
//         {
//             *(rt_uint8_t *)args = NDS03_DEFAULT_SLAVE_ADDR;
//         }
//         break;
//     case RT_SENSOR_CTRL_SET_POWER:
//     {
//         rt_uint32_t pctrl = (rt_uint32_t)args;
//         if (pctrl == RT_SENSOR_POWER_DOWN)
//         {
//             nds03_power_down(pNxDevice);
//             LOG_D("NDS03 power down");
//         }
//         else
//         {
//             result = nds03_continuous_ranging_init(pNxDevice);
//             LOG_D("NDS03 power up");
//         }
//     }
//     break;
//     default:
//         break;
//     }
//     return result;
// }
// static rt_size_t nds03_fect_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len)
// {
//     struct rt_sensor_data *data = (struct rt_sensor_data *)buf;
//     NDS03_Dev_t *pNxDevice = sensor->parent.user_data;
//     if (sensor->info.type == RT_SENSOR_CLASS_TOF)
//     {
//         /* 获取测量数据 */
//         NDS03_GetContinuousRangingData(pNxDevice);
//         data->timestamp = rt_sensor_get_ts();
//         data->type = RT_SENSOR_CLASS_TOF;
//         data->data.tof.confi = pNxDevice->ranging_data[0].confi;
//         data->data.tof.count = pNxDevice->ranging_data[0].count;
//         data->data.tof.crate = pNxDevice->ranging_data[0].crate;
//         data->data.tof.depth = pNxDevice->ranging_data[0].depth;

//     // NX_PRINTF("depth1:%4d mm, confi:%4d, count:%4d\r\n", pNxDevice->ranging_data[0].depth, pNxDevice->ranging_data[0].confi, pNxDevice->ranging_data[0].count);
//     // NX_PRINTF("depth2:%4d mm, confi:%4d, count:%4d \t", pNxDevice->ranging_data[1].depth, pNxDevice->ranging_data[1].confi, pNxDevice->ranging_data[1].count);
//     // NX_PRINTF("depth3:%4d mm, confi:%4d, count:%4d \t", pNxDevice->ranging_data[2].depth, pNxDevice->ranging_data[2].confi, pNxDevice->ranging_data[2].count);
//     // NX_PRINTF("depth4:%4d mm, confi:%4d, count:%4d \t", pNxDevice->ranging_data[3].depth, pNxDevice->ranging_data[3].confi, pNxDevice->ranging_data[3].count);

//     }
//     return 1;
// }

// static struct rt_sensor_ops nds03_ops =
// {
//     nds03_fect_data,
//     nds03_control,
// };

// int init_tof_sensor(void)
// {
//     rt_int8_t result;
//     rt_int8_t tryI2c=0;
//     rt_sensor_t sensor = RT_NULL;
//     static NDS03_Dev_t g_nds03_device = {
//         .platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR,
//         .platform.xshut_pin = GET_PIN(B, 6),
//     };
//     while(1)
//     {
//         NDS03_Wakeup(&g_nds03_device);
//         rt_thread_mdelay(1);
//         if (i2c_device_try("i2c2", NDS03_DEFAULT_SLAVE_ADDR) == 1)
//         {
//             result = nds03_continuous_ranging_init(&g_nds03_device);
//             if(result == NDS03_ERROR_NONE)
//             {
//                 nds03_power_down(&g_nds03_device);
//                 break;
//             }
//         }
//         // rt_thread_mdelay(1);
//         if( tryI2c++ > 3)
//         {
//             LOG_E("can't find NDS03 sensor");
//             return -1;
//         }
//     }
//     sensor = rt_calloc(1, sizeof(struct rt_sensor_device));
//     if(RT_NULL == sensor)
//     {
//         LOG_E("fail to rt_calloc");
//         return -1;
//     }

//     sensor->info.type = RT_SENSOR_CLASS_TOF;
//     sensor->info.vendor = RT_SENSOR_VENDOR_MERRILLCHIP;
//     sensor->info.model = "NDS03";
//     sensor->info.unit = RT_SENSOR_UNIT_MM;
//     sensor->info.intf_type = RT_SENSOR_INTF_I2C;
//     sensor->info.range_max = 4500;
//     sensor->info.range_min = 0;
//     sensor->info.period_min= 33;
//     sensor->info.fifo_max = 0;

//     sensor->ops = &nds03_ops;

//     result = rt_hw_sensor_register(sensor, "NDS03", RT_DEVICE_FLAG_RDWR, &g_nds03_device);

//     return result;
// }
// ZINO_DEVICE_EXPORT(init_tof_sensor);
